package com.DataProcess;

import android.util.Log;

import java.util.Arrays;

/**
 * 功能：处理UWB数据，当刷新的有效个数达到后之后，返回ready
 * Created by Cyc on 2016/8/25 0025.
 */
public class UWBDataProcess {
    private String thisLogTag = "UWBDataProcess";

    public double[] distance;
    public double[] distance_old;
    private boolean isFirstTime =  true;
    private int readyThreshold = 4;   //ready的有效个数设置

    public UWBDataProcess(int num) {
        setAmount(num);
    }

    public void setAmount(int num) {
        isFirstTime = true;
        distance = new double[num];
        distance_old = new double[num];
        reset();
        System.arraycopy(distance,0,distance_old,0,num);
    }

    public void setReadyCount(int num) {
        readyThreshold = num;
    }

    public boolean putDistance(int id, short distance) {
        notReceiveCnt = 0;
        isNotReceive = false;
        Log.i(thisLogTag,"isNotReceive " + isNotReceive + "notReceiveCnt " + notReceiveCnt);
        if(distance < 0.0001)
            return false;

        if (this.distance != null && this.distance.length >= id) {
            this.distance[id] = distance/100.0;
            int count = 0;
            for(int i=0; i<this.distance.length; i++) {
                if(this.distance[i] < 100.0)
                    count++;
            }
            if (count >= readyThreshold) {
                Log.i(thisLogTag, "uwb distance:"+ Arrays.toString(this.distance));
                return true;
            }
        }

        return false;
    }


    public void filterErr() {
    }
    
    public void reset() {
        for (int i = 0; i < distance.length; i++) {
            distance[i] = 100.0;
        }
    }

    public int notReceiveCnt = 0;
    public boolean isNotReceive = false;
    public boolean isDataWrong = false;
    public void checkUWBReceive() {
        notReceiveCnt ++;
        if (notReceiveCnt > 3) {
            Log.i(thisLogTag,"CAN didn't receive UWB in "+notReceiveCnt+ "times");
        }
        if (notReceiveCnt > 12) {
            isNotReceive = true;
            Log.e(thisLogTag,"CAN didn't receive UWB in " + notReceiveCnt + " times");
        }
        for(int i=0; i<distance.length; i++) {
            if(distance[i] < 0.2){ //UWB数据过近
                isDataWrong = true;
                Log.i(thisLogTag,"uwb data wrong");
                break;
            }
            isDataWrong = false;
        }
    }
}
